/*
Copyright (c) 2021, WN90 18010090131@126.com
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

// SGM算法的金字塔缩放优化
// 速度确实提升了不少，但是质量还是不够，层数多了，小物体可能会丢失，
// 为了不让小物体丢失，去掉了最开始设计的各种复杂的视差范围确定， 匹配有问题就重来，
// 为了减少重来的计算量，没有使用整个视差范围，而是根据左右点的视差的约束，确定一个小的范围
//
// 实际上最主要的提速原因并不是采用了多层结构，而是topn模型，
// 即 代价计算的时候只保留topn个，这样在聚合的时候计算量就
// 极大的降低了，相邻层间聚合计算量和 drange*drange 成正比
//
// 多层的优点是不用事先确定视察范围，全范围搜索，如果没有多层
// 全范围还是会稍稍慢一点，在计算能力低的设备上多层应该会有优势
//
// 使用了大顶堆来实现topn，本文没有使用c++ STL的heap，因为是c数组结构
// 后来测试通过数组指针，也可以使用STL。
//

#include <cmath>
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/base.hpp>
#include <opencv2/core/matx.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <array>
#include <string>
#include <type_traits>

#include <bits/stdc++.h>
#include <stdexcept>

#include <cstring>

#include <chrono>
#include <climits>

#include <algorithm>
#include <cinttypes>
#include <memory>

#include <chrono>

using namespace std;
using namespace cv;

// 有效像素再-2
#define NUM_PIXEL_MIN 12
#define MIN_DISP 6

#define INVALID_VAL ((DispSize_t)INT_MAX)
#define OCCLUDED (INVALID_VAL - 1)
// < VALID_MAX是合法视差
#define VALID_MAX (OCCLUDED - 2)
#define COST_MAX ((CostSize_t)INT_MAX)
#define arr_access(arr, jstep, kstep, i, j, k) ((arr)[((i) * (jstep) * (kstep)) + ((j) * (kstep)) + (k)])

#define likely(x) __builtin_expect(!!(x), 1)
#define unlikely(x) __builtin_expect(!!(x), 0)

typedef uint16_t DispSize_t;
typedef uint16_t CostSize_t;
typedef uint16_t Census_t;
typedef int16_t  Grad_t;

// 这里没有使用通常使用的数组形式保存不同d的代价，所以要把d和cost放在一起，这样才知道cost对应的是那个d
struct DispCost_t {
	CostSize_t  cost;
	DispSize_t  disp;
	inline bool operator<(DispCost_t& right) const {
		return this->cost < right.cost;
	}
	inline bool operator>(DispCost_t& right) const {
		return this->cost > right.cost;
	}
	inline bool operator<=(DispCost_t& right) const {
		return this->cost <= right.cost;
	}
	inline bool operator>=(DispCost_t& right) const {
		return this->cost >= right.cost;
	}
	inline bool operator==(DispCost_t right) const {
		return this->cost == right.cost;
	}
	inline bool operator!=(DispCost_t right) const {
		return this->cost != right.cost;
	}
	inline DispCost_t& operator+=(DispCost_t right) {
		assert(this->disp == right.disp);
		this->cost += right.cost;
		return *this;
	}
	inline DispCost_t& operator+=(CostSize_t right) {
		this->cost += right;
		return *this;
	}
	inline DispCost_t& operator-=(DispCost_t right) {
		assert(this->disp == right.disp);
		this->cost -= right.cost;
		return *this;
	}
	inline DispCost_t& operator-=(CostSize_t right) {
		this->cost -= right;
		return *this;
	}
	template <typename tn>
	inline DispCost_t operator+(const tn& right) const {
		DispCost_t result(*this);
		result += right;
		return result;
	}
	template <typename tn>
	inline DispCost_t operator-(const tn& right) const {
		DispCost_t result(*this);
		result -= right;
		return result;
	}
};

class PySgm {
public:
	PySgm(int height, int width, int cost_num, CostSize_t P1, CostSize_t P2, int P1_threshold, int level_num) :
		PySgm(height, width, cost_num, P1, P2, P1_threshold, 0, level_num){};
	PySgm(int height, int width, int cost_num, CostSize_t P1, CostSize_t P2, int P1_threshold, int level, int level_num);

	// 范围: [start, end)，如果end过大，会缩放到图像长度再小一点，cut_offset用于对最终结果调整，如果图像裁剪过，左侧两图裁剪不一致，需要增加偏移
	// 目前不支持负数视差，所以左图左侧裁剪要比右图左侧裁剪小，所以cut_offset只能为负数
	void match(const Mat& l, const Mat r, int start, int end, Mat& result, int cut_offset = 0);

private:
	int height;
	int width;

	// 下采样的时候如果不整除，左图左边留一个不用，右图右边留一个不用，上下方向在上侧留一个不用
	int hoffset;
	int woffset;

	// topn个最小的代价，一共cost_num-1个，留一个头保存个数
	int cost_num;

	// SGM算法中的P1 P2，这里做了分段线性的修改
	CostSize_t P1 = 1;
	CostSize_t P2 = 16;
	// P1惩罚的范围 <= P1_threshold
	int P1_threshold = 4;

	// 下面两个差8,是为了用移位运算提速，因为大量重复计算，效果比较明显
	// 针对的是像素梯度，而不是别的
	// P2惩罚开始降低的点
	const int P2_threshold1 = 11;
	// P2惩罚降低到P1惩罚的点
	const int P2_threshold2 = 19;

	// 当前属于哪一层,最上层是0
	int level;

	// const int blocksize = 3;
	const int offset = 1;

	std::unique_ptr<uint8_t[]> l = nullptr;
	std::unique_ptr<uint8_t[]> r = nullptr;
	// 计划是拿出一部分作为小数，但是没有实现，都用作整数了，也就是视差不是小数
	// 使用小数的办法是，整体放大128倍或64倍，高清图像16位可能不够用
	std::unique_ptr<DispSize_t[]> ldisp = nullptr;
	std::unique_ptr<DispSize_t[]> rdisp = nullptr;

	// 上下传递用，小层给本层暗示，本层更新，然后传递给更大层
	// 已经不用了，如果使用更复杂的判断逻辑可能会需要
	// std::unique_ptr<DispSize_t[]> ldisp_range = nullptr;

	// 左右census的结果
	std::unique_ptr<Census_t[]> lcensus = nullptr;
	std::unique_ptr<Census_t[]> rcensus = nullptr;
	// 左右梯度， 第一维0 1对应dx和dy
	std::unique_ptr<Grad_t[]> lgrad = nullptr;
	std::unique_ptr<Grad_t[]> rgrad = nullptr;

	// 以左图为基准计算的代价
	std::unique_ptr<DispCost_t[]> cost = nullptr;

	// x方向左右两次聚合公用一个，y类似
	std::unique_ptr<DispCost_t[]> laggr_x = nullptr;
	std::unique_ptr<DispCost_t[]> laggr_y = nullptr;

	// 没有针对右图计算，只是借用了左图的结果
	std::unique_ptr<DispCost_t[]> raggr_x = nullptr;
	// std::unique_ptr<DispCost_t[]> raggr_y = nullptr;

	// 聚合之后结果相加，注意这里是引用，复用raggr_x节约内存。
	std::unique_ptr<DispCost_t[]>& laggr_sum;
	std::unique_ptr<DispCost_t[]>& raggr_sum;

	// 计算代价的时候，根据lower层传递过来的估计和视差范围确定需要计算的d都有哪些
	inline void range_get(int i, int j, int& start, int& end, bool is_lowest);
	// 最后一步，获取结果
	void disparity_get(bool is_lowest);
	// lower层结果向上传递
	void disp_upscale_from_next(int disp_start, int disp_end);
	// 图像下采样
	void downscale_to_next(void);
	// 计算p2，这里对性能影响比较大
	inline int calc_p2(int d_diff, int pixel_diff);

	void grad_calc(void);
	void census_calc(void);

	// 代价计算
	void cost_get(int disp_start, int disp_end, bool is_lowest);
	// 视差计算
	void match(int start, int end);
	// 代价聚合，本来是打算支持左右分别计算，所以接口复杂一点，
	// 后来，把获取右侧的代价结果(借用左侧)也放到这里了，也就不支持左右分别计算了
	void cost_aggr(std::unique_ptr<DispCost_t[]>& cost,
				   std::unique_ptr<DispCost_t[]>& aggr_x,
				   std::unique_ptr<DispCost_t[]>& aggr_y,
				   std::unique_ptr<uint8_t[]>&    image,
				   std::unique_ptr<DispCost_t[]>& sum);

	// 和opencv的转换
	void load_from_opencv(const Mat& l, const Mat& r);
	void disp_to_opencv(Mat& result, int cut_offset);
	void grad_to_opencv(Mat& img0, Mat& img1);
	void img_to_opencv(Mat& img0, Mat& img1);
	void img_to_opencv(Mat& img0, Mat& img1, int channel);

	// 下一层
	std::unique_ptr<PySgm> next = nullptr;
};

PySgm::PySgm(int height, int width, int cost_num, CostSize_t P1, CostSize_t P2, int P1_threshold, int level, int level_num) :
	height(height), width(width), cost_num(cost_num), P1(P1), P2(P2), P1_threshold(P1_threshold), level(level), laggr_sum(laggr_x), raggr_sum(raggr_x) {
	// 节约点空间，已经在上面了
	// laggr_sum = laggr_x;
	// raggr_sum = raggr_x;

	if (level == 0) {
		if (level_num < 1 || level_num > 20)
			throw std::range_error("wrong level_num");

		int minlen = (NUM_PIXEL_MIN << (level_num - 1)) + (1 << ((level_num - 1) + 1)) - 2;
		if (width < minlen || height < minlen)
			throw std::range_error("min{width, height} < (10<<(level_num-1)) + (1<<((level_num-1)+1)) - 2");
	}

	cout << "initializing level " << level << endl;

	// 初始化下一层
	if (level_num > 1) {
		auto next_height = (height - 4) / 2 + 1;
		auto next_width = (width - 4) / 2 + 1;
		hoffset = (height - 4) % 2;
		woffset = (width - 4) % 2;
		int next_P1_threshold = std::max(1, P1_threshold / 2);
		next = std::make_unique<PySgm>(next_height, next_width, cost_num, P1, P2, next_P1_threshold, level + 1, level_num - 1);
	} else {
		hoffset = 0;
		woffset = 0;
		next = nullptr;
	}

	l = std::make_unique<uint8_t[]>(3 * height * width);
	r = std::make_unique<uint8_t[]>(3 * height * width);
	ldisp = std::make_unique<DispSize_t[]>(height * width);
	rdisp = nullptr;
	// ldisp_range = std::make_unique<DispSize_t[]>(height_ * width_ * 2);

	lcensus = std::make_unique<Census_t[]>(height * width);
	rcensus = std::make_unique<Census_t[]>(height * width);

	lgrad = std::make_unique<Grad_t[]>(2 * (height - 1) * (width - 1));
	rgrad = std::make_unique<Grad_t[]>(2 * (height - 1) * (width - 1));

	cost = std::make_unique<DispCost_t[]>(height * width * cost_num);
	laggr_x = std::make_unique<DispCost_t[]>(height * width * cost_num);
	laggr_y = std::make_unique<DispCost_t[]>(height * width * cost_num);

	raggr_x = std::make_unique<DispCost_t[]>(height * width * cost_num);

	if (level == 0) {
		// 多了，实际上只初始化最边缘一层就可以了
		for (int i = 0; i < height * width; i++) {
			ldisp[i] = INVALID_VAL;
		}
	}
}

void PySgm::match(const Mat& l, const Mat r, int start, int end, Mat& result, int cut_offset) {
	auto ts = std::chrono::steady_clock::now();

	load_from_opencv(l, r);

	auto te = std::chrono::steady_clock::now();
	auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(te - ts);
	cout << "load from opencv:\t" << duration.count() / 1000. << endl;
	ts = te;

	match(start, end);

	te = std::chrono::steady_clock::now();
	duration = std::chrono::duration_cast<std::chrono::milliseconds>(te - ts);
	cout << "total match:\t" << duration.count() / 1000. << endl;
	ts = te;

	disp_to_opencv(result, cut_offset);

	te = std::chrono::steady_clock::now();
	duration = std::chrono::duration_cast<std::chrono::milliseconds>(te - ts);
	cout << "to opencv:\t" << duration.count() / 1000. << endl;
}

// [start, end)
void PySgm::match(int start, int end) {
	assert(end > start && start >= 0);
	// 两边空一条
	if (end > width - 2 * offset)
		end = width - 2 * offset;

#if 0
	Mat img0, img1;
	img_to_opencv(img0, img1, 0);
	imshow(std::to_string(level) + "l", img0);
	imshow(std::to_string(level) + "r", img1);
	waitKey();
#endif

	cout << "match start end :" << start << " " << end << endl;

	// 范围上百分之百能还原，无论是否整除
	int next_start = start / 2;
	int next_end = end / 2 + 1;

	bool is_lowest = true;

	// 匹配lower层
	if (next_end - next_start > MIN_DISP && next != nullptr) {
		is_lowest = false;
		downscale_to_next();
		next->match(next_start, next_end);
		disp_upscale_from_next(start, end);
	}

	cout << "---------------level:" << level << endl;

	auto ts = std::chrono::steady_clock::now();

	grad_calc();
	census_calc();
	cost_get(start, end, is_lowest);
	cost_aggr(cost, laggr_x, laggr_y, l, laggr_sum);
	disparity_get(is_lowest);

	auto te = std::chrono::steady_clock::now();
	auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(te - ts);
	cout << "time used:\t" << duration.count() / 1000. << endl;
}

// is_big 控制大顶堆或小顶堆
#define swap(a, b)       \
	do {                 \
		typeof(a) t = a; \
		a = b;           \
		b = t;           \
	} while (0)
static inline void heap_siftup(DispCost_t arr[], int cur, bool is_big) {
	if (cur < 1)
		return;

	int parent = (cur - 1) >> 1;

	if ((!is_big && arr[parent] > arr[cur])
		|| (is_big && arr[parent] < arr[cur])) {
		swap(arr[parent], arr[cur]);
		return heap_siftup(arr, parent, is_big);
	}
}

// 不包括end
static inline void heap_siftdown(DispCost_t arr[], int cur, int end, bool is_big) {
	int left_son = 2 * cur + 1;
	int right_son = left_son + 1;
	int target_son = left_son;

	if (left_son >= end)
		return;

	if (right_son < end) {
		if ((is_big && arr[right_son].cost > arr[left_son].cost)
			|| (!is_big && arr[right_son].cost < arr[left_son].cost))
			target_son = right_son;
	}

	if (is_big && arr[cur].cost >= arr[target_son].cost)
		return;

	if (!is_big && arr[cur].cost <= arr[target_son].cost)
		return;

	swap(arr[target_son], arr[cur]);

	return heap_siftdown(arr, target_son, end, is_big);
}

// 必须已经是堆
static inline void heap_sort(DispCost_t arr[], int num, bool is_big) {
	for (int i = num - 1; i > 0; i--) {
		swap(arr[0], arr[i]);
		heap_siftdown(arr, 0, i, is_big);
	}
}

static inline void __heap_append(DispCost_t arr[], int end, DispCost_t new_data, bool is_big) {
	arr[end] = new_data;
	heap_siftup(arr, end, is_big);
}
static inline int heap_size(DispCost_t head) {
	assert(head.disp == (DispSize_t)-1);
	return head.cost;
}
static inline void heap_init(DispCost_t& head) {
	// 第一个作为head，cost为元素个数
	head.cost = 0;
	head.disp = (DispSize_t)-1;
}
// 确保有数据才能peek
static inline DispCost_t heap_peek(DispCost_t* head) {
	assert(heap_size(*head) > 0);
	return head[1];
}
static inline bool heap_pop(DispCost_t head[], DispCost_t& output, bool is_big) {
	auto& size = head[0].cost;
	if (size == 0)
		return false;

	auto arr = &head[1];
	output = arr[0];

	// 最后的填充到前面，优先队列的方式
	arr[0] = arr[size - 1];
	size--;
	assert(size >= 0);

	heap_siftdown(arr, 0, size, is_big);
	return true;
}

static inline void heap_insert(DispCost_t head[], int len, DispCost_t new_data, bool is_big) {
	auto& size = head[0].cost;
	assert(size == heap_size(head[0]));
	assert(head[0].disp == (DispSize_t)-1);

	// 还没满
	if (size < len - 1) {
		__heap_append(&head[1], size, new_data, is_big);
		size++;
		assert(size == head[0].cost);
		return;
	}

	// 保存前n个最小的
	auto& max = head[1];
	if (is_big && new_data.cost >= max.cost) {
		return;
	}
	if (!is_big && new_data.cost <= max.cost) {
		return;
	}

	// head[1]是开始
	max = new_data;
	assert(head[1].cost == max.cost);
	assert(head[1].disp == max.disp);
	heap_siftdown(&head[1], 0, size, is_big);
}

void test_heap(void) {
	const int  arrsize = 10;
	DispCost_t a[arrsize], b[arrsize];
	memset(a, 0, arrsize * sizeof(DispCost_t));
	memset(b, 0, arrsize * sizeof(DispCost_t));
	heap_init(a[0]);
	heap_init(b[0]);

#if 0
	auto printheap = [](DispCost_t *arr){
		for (int i = 0 ; i < heap_size(arr[0]); i++){
			cout << arr[i+1].cost << "\t";
		}
		cout << endl;
	};
#endif
	assert(heap_size(a[0]) == 0);
	int min = 1, max = 59;
	for (auto i = min; i < max + 1; i++) {
		heap_insert(a, arrsize, {(DispSize_t)(i), (CostSize_t)i}, true);
		heap_insert(b, arrsize, {(DispSize_t)(i), (CostSize_t)i}, false);
		// printheap(b);
		// cout << i << endl;
		// cout << heap_size(a[0]) << endl;
		// cout << heap_size(b[0]) << endl;
		assert(heap_size(a[0]) == std::min(i, arrsize - 1) && heap_size(a[0]) == heap_size(b[0]));
	}
	int len = heap_size(a[0]);
	(void)len;
	for (int i = 0; i < 20; i++) {
		DispCost_t ra;
		DispCost_t rb;
		bool       reta = heap_pop(a, ra, true);
		bool       retb = heap_pop(b, rb, false);
		(void)retb;
		assert(reta == retb);
		if (reta) {
			cout << ra.cost << "\t" << rb.cost << endl;
			assert(heap_size(a[0]) == std::max(len - 1 - i, 0) && heap_size(a[0]) == heap_size(b[0]));
			assert(ra.cost == min + arrsize - 1 - 1 - i);
			assert(rb.cost == max - (arrsize - 1 - 1) + i);
		}
	}
	assert(heap_size(a[0]) == 0 && heap_size(a[0]) == heap_size(b[0]));
	array<int, 100> data;
	array<int, 100> data2;
	for (int i = 0; i < 100; i++) {
		data[i] = rand() % 100;
		data2[i] = data[i];
		heap_insert(a, arrsize, {(DispSize_t)(data[i]), (CostSize_t)data[i]}, true);
		heap_insert(b, arrsize, {(DispSize_t)(data[i]), (CostSize_t)data[i]}, false);
	}
	sort(data.begin(), data.end());

	int* pa = &data[0] - 1;
	int* pb = &data[data.size()];
	pa += (arrsize - 1);
	pb -= (arrsize - 1);

	cout << "------------------" << endl;
	for (int i = 0; i < 20; i++) {
		DispCost_t ra;
		DispCost_t rb;
		bool       reta = heap_pop(a, ra, true);
		bool       retb = heap_pop(b, rb, false);
		(void)retb;
		assert(reta == retb);
		if (reta) {
			cout << ra.cost << "\t" << rb.cost << endl;
			assert(heap_size(a[0]) == std::max(len - 1 - i, 0) && heap_size(a[0]) == heap_size(b[0]));
			assert(ra.cost == (*pa--));
			assert(rb.cost == (*pb++));
		}
	}
	for (int i = 0; i < arrsize - 1; i++) {
		heap_insert(a, arrsize, {(DispSize_t)data2[i], (DispSize_t)data2[i]}, true);
		heap_insert(b, arrsize, {(DispSize_t)data2[i], (DispSize_t)data2[i]}, false);
	}
	heap_sort(a, arrsize, true);
	heap_sort(b, arrsize, false);
	for (int i = 0; i < arrsize - 1; i++) {
		cout << a[i].cost << endl;
		assert(a[i] == b[arrsize - 2 - i]);
	}
}

// 差的绝对值
template <typename T>
static inline T diff_abs(T a, T b) {
	if (a > b)
		return a - b;
	return b - a;
}

void PySgm::cost_get(int disp_start, int disp_end, bool is_lowest) {
	// 多一个用于记录大小，最多保存七个最大

	auto img = [&](std::unique_ptr<uint8_t[]>& img, int i, int j, int k) -> int {
		return arr_access(img, height, width, i, j, k);
	};
	//	auto grad = [&](std::unique_ptr<Grad_t[]> &grad, int xy, int i, int j) -> Grad_t {
	//		return arr_access(grad, (height-1), (width-1), xy, i, j);
	//	};
	auto census = [&](std::unique_ptr<Census_t[]>& c, int i, int j) -> Census_t {
		return arr_access(c, height, width, 0, i, j);
	};

	auto disp_cost = [&](std::unique_ptr<DispCost_t[]>& c, int i, int j) -> DispCost_t& {
		return arr_access(c, width, cost_num, i, j, 0);
	};

	auto Grad = [&](std::unique_ptr<Grad_t[]>& grad, int xy, int i, int j) -> Grad_t& {
		return arr_access(grad, (height - 1), (width - 1), xy, i, j);
	};

	// 无论梯度，还是census，最外层都是不准确的，为了保证准确性，四周留出offset
	for (int i = 0 + offset; i < height - offset; i++) {
		for (int j = width - offset - 1; j >= 0 + offset; j--) {
			// 未赋值的为随机值，不能使用
			auto cost_arr = &disp_cost(this->cost, i, j);
			heap_init(cost_arr[0]);

			int start = disp_start, end = disp_end;
			range_get(i, j, start, end, is_lowest);

			assert(disp_start <= start && start <= end && end <= disp_end);

			// 左图左侧的点的视差有约束，要保证在右图有对应，最左侧(offset)只能为0
			int dmax = std::min((int)end, j - offset + 1);
			for (int d = start; d < dmax; d++) {
				int cost = 0;
#if 1
				{
					auto bits = census(lcensus, i, j) ^ census(rcensus, i, j - d);

					
					while (bits) {
						cost++;
						bits = bits & (bits - 1);
					}
				}
#endif

				// 提前排除，减少后面的计算量
				if (cost < 12) {
					int diff_right = diff_abs(Grad(lgrad, 0, i, j), Grad(rgrad, 0, i, j - d));
					int diff_left = diff_abs(Grad(lgrad, 0, i, j - 1), Grad(rgrad, 0, i, j - d - 1));

					// 排除遮挡以及y方向视察变化造成的梯度差异较大的现象，其中一侧是正常的，一侧是和遮挡等相关的，所以取最小
					// 到底有没有效果，或者效果多大，还没有深入研究
					// int gradx_cost = (diff_left + diff_right)>>1;
					int gradx_cost = std::min(diff_left, diff_right);

					// 梯度主要看纹理变化，/太大了，就看不到很多变化了，/太小了，又会有随机干扰
					// [0, 64]
					// 类似四舍五入，后面的除法都类似
					gradx_cost = (gradx_cost + 2) / 4;

					gradx_cost = 0;

					if (gradx_cost < 16) {
						cost += gradx_cost;

						int diff_down = diff_abs(Grad(lgrad, 1, i, j), Grad(rgrad, 1, i, j - d));
						int diff_up = diff_abs(Grad(lgrad, 1, i - 1, j), Grad(rgrad, 1, i - 1, j - d));
						int grady_cost = std::min(diff_up, diff_down);
						// int grady_cost = (diff_up + diff_down) >> 1;
						grady_cost = (grady_cost + 2) / 4;

						// 屏蔽
						grady_cost = 0;

						if (grady_cost < 16) {
							cost += grady_cost;

							// YUV/YCrCb空间，经过简单观察，颜色通道受亮度变化所造成的影响小，和BGR相比。
							// 范围可调，uv变化 看起来不大，y波动比较大，如果y不缩放，census受干扰就太强了
							// 范围虽大，但正确匹配的数值一般都比较小
							//
							// y [0, 16]
							// auto ycost = (diff_abs(img(l, 0, i, j), img(r, 0, i, j-d)) +8) / 16;
							// if(ycost <= 10){
							auto ycost = (diff_abs(img(l, 0, i, j), img(r, 0, i, j - d)) + 2) / 4;

							// 屏蔽
							// ycost = 0;

							if (ycost <= 9) {
								cost += ycost;
								// u
								auto ucost = (diff_abs(img(l, 1, i, j), img(r, 1, i, j - d)) + 8) / 16;

								// 屏蔽
								ucost = 0;

								if (ucost <= 8) {
									cost += ucost;
									// v
									auto vcost = (diff_abs(img(l, 2, i, j), img(r, 2, i, j - d)) + 2) / 4;

									// 屏蔽
									vcost = 0;

									if (vcost <= 8) {
										cost += vcost;
										DispCost_t dc;
										dc.disp = d;
										dc.cost = cost;
										heap_insert(cost_arr, cost_num, dc, true);
										assert(heap_size(cost_arr[0]) < cost_num);
									}
								}
							}
						}
					}
				}
			}
		}
	}
}

inline void PySgm::range_get(int i, int j, int& start, int& end, bool is_lowest) {
	// assert(ldisp_range != nullptr);
	if (unlikely(is_lowest))
		return;

	assert(i >= 1 && i < height - 1);
	assert(j >= 1 && j < width - 1);

	// 边界lower层的传递的结果无效
	const int border_size = 2 * offset + 1;
	if ((i < hoffset + border_size || i >= height - border_size)
		|| (j < woffset + border_size || j >= width - border_size)) {
		return;
	}
	assert(start + 2 <= end);
	auto Disp = [&](std::unique_ptr<DispSize_t[]>& disp, int i, int j) -> DispSize_t& {
		return arr_access(disp, height, width, 0, i, j);
	};
	int min = VALID_MAX;
	int max = -1;

	int d = Disp(ldisp, i, j);
	if (d < VALID_MAX) {
		min = d;
		max = d;
	}

	// 全范围匹配时，会提升速度
	if (max == -1) {
		// 遮挡点这个公式不成立
		// dr - (xr-x) <= d <= dl + x-xl
		// 先找右侧确定min再找左侧确定max
		int       count = 0;
		const int delta = 1 + (3 >> (level >> 1)); // std::min(9,  1 + ((end-start) >> 3));

		// 间隔查找，避免碰到一片连续的错误，查找到的可能无效，所以查找多次
		for (int xr = j + delta; xr < width - offset; xr += delta) {
			auto dr = Disp(ldisp, i, xr);
			if (dr < VALID_MAX) {
				// 可能是负数
				min = std::min(min, dr + j - xr);
				count++;
				if (count == 6 || min <= start)
					break;
			}
		}

		count = 0;
		for (int xl = j - delta; xl >= offset; xl -= delta) {
			auto dl = Disp(ldisp, i, xl);
			if (dl < VALID_MAX) {
				max = std::max(max, dl + j - xl);
				count++;
				if (count == 6 || max >= end)
					break;
			}
		}
	}

	// const int step = (2 << (level <= 1));
	//  level大计算量小，范围可以大一点，降低误差
	const int step = 1 + level;

	// 上下方向的邻居的视差范围
	int min_ud = VALID_MAX;
	int max_ud = -1;
	int du = Disp(ldisp, i - 1, j);
	if (du < VALID_MAX) {
		min_ud = du;
		max_ud = du;
	}
	int dd = Disp(ldisp, i + 1, j);
	if (dd < VALID_MAX) {
		min_ud = std::min(min_ud, dd);
		max_ud = std::max(max_ud, dd);
	}

	if (unlikely(min == VALID_MAX))
		min = start;
	else {
		min = std::min(min, min_ud);
		min = ((min > start + step)) ? (min - step) : start;
	}

	if (unlikely(max == -1))
		max = end;
	else {
		max = std::max(max, max_ud);
		max = ((max + step < end)) ? (max + step) : end;
	}

	if (unlikely(min > max)) {
		if (min > end)
			min = max - 1;
		else
			max = min + 1;
	}
	start = min;
	end = max;
	assert(start <= end);
	return;
}

// 不仅仅是计算p2, 也包含一部分p1，倾斜严重的平面视差相差可能不止1,
// 所以p1部分做了扩展，但是视差差越大，惩罚也越大，只是不过大
// 本函数使用除法或过复杂会影响一点速度
inline int PySgm::calc_p2(int d_diff, int pixel_diff) {
	assert(d_diff > 0);

	// p1 * 2^((threshold-1)/2) + p1 * 2^(threshold/4) - p1
	// 对于 threshold= 1 2 3 4 5 6 , 结果: 1*p1 1*p1 2*p1 3*p1 5*p1 5*p1
	// P1 1,P1_threshold 4   =>  0 1 2 4 7
	// ((P1 << ((P1_threshold-1)>>1)) + (P1 << (P1_threshold>>2)) - P1);

	// 梯度大，可能是边界，最小惩罚
	if (pixel_diff >= P2_threshold2)
		return P1;

	int p;
	if (d_diff <= P1_threshold)
		p = (((((P1 << 1) - 1) >> 2) + d_diff)); // p1部分
	else
		p = P2;

	assert(P2 >= (((((P1 << P1_threshold) - 1) >> 2) + P1_threshold)));

	// 直线连接两个端点 P2_threshold1 P2_threshold2，一个P1,一个p，求中间的
	if (pixel_diff > P2_threshold1) {
		// p = p+ (pixel_diff - P2_threshold1) *(P1-p)/(P2_threshold2- P2_threshold1);
		int inc = (pixel_diff - P2_threshold1) * (P1 - p);
		assert(P2_threshold2 - P2_threshold1 == 8);
		inc >>= 3; // /8
		p = p + inc;
	}

	assert(p > 0);
	return p;
}

void PySgm::disparity_get(bool is_lowest) {
	auto Head = [&](std::unique_ptr<DispCost_t[]>& c, int i, int j) -> DispCost_t* {
		return &arr_access(c, width, cost_num, i, j, 0);
	};
	auto Disp = [&](std::unique_ptr<DispSize_t[]>& disp, int i, int j) -> DispSize_t& {
		return arr_access(disp, height, width, 0, i, j);
	};

	// 边缘无计算结果的地方填充为无效
	// level == 0，Disp也不需要填充，初始化的时候填充了
	if (level != 0) {
		for (int i = 0; i < offset; i++) {
			for (int j = 0; j < width; j++) {
				Disp(ldisp, i, j) = INVALID_VAL;
			}
		}
		for (int i = offset; i < height - offset; i++) {
			for (int j = 0; j < offset; j++) {
				Disp(ldisp, i, j) = INVALID_VAL;
			}
			for (int j = width - offset; j < width; j++) {
				Disp(ldisp, i, j) = INVALID_VAL;
			}
		}
		for (int i = height - offset; i < height; i++) {
			for (int j = 0; j < width; j++) {
				Disp(ldisp, i, j) = INVALID_VAL;
			}
		}
	}

	for (int i = 0 + offset; i < height - offset; i++) {
		for (int j = 0 + offset; j < width - offset; j++) {
			// 最小的层，初始视差应该为无效
			if (unlikely(is_lowest))
				Disp(ldisp, i, j) = INVALID_VAL;

			DispCost_t* lcost = Head(laggr_sum, i, j);

			bool       has_best_match = false;
			int        best_disp_num = 0;
			DispSize_t best_disp[cost_num];
			CostSize_t best_cost = COST_MAX;

			DispCost_t dc = {0, 0};
			if (heap_pop(lcost, dc, false)) {
				best_cost = dc.cost;
			}

			// 代价最低的，可能有多个，每一个都看和右侧的结果，看右侧对应的最佳和自己是不是差1以内
			// 即SGM中说的左右检查
			while (best_cost == dc.cost) {
				DispCost_t* rcost = Head(raggr_sum, i, j - dc.disp);
				DispCost_t  rdc = heap_peek(rcost);

				// 此处相当于了left +-1，和右图对应
				// 容量有限(甚至一个)，相同的不会全保存，所以只要cost相同就算匹配了
				// 和最佳匹配差1,也算匹配了
				if (/*dc.cost != rdc.cost && */ diff_abs(dc.disp, rdc.disp) > 1) {
				} else {
					best_disp[best_disp_num++] = dc.disp;
					has_best_match = true;
				}

				if (!heap_pop(lcost, dc, false)) {
					break;
				}
			}

			if (has_best_match) {
				assert(best_disp_num > 0);
				if (best_disp_num == 1) {
					Disp(ldisp, i, j) = best_disp[0];
				} else {
					// 选择和lower传递过来的结果最近的，
					auto hint = Disp(ldisp, i, j);
					// 0 关闭这个操作，考虑的是只要有差别大一点的结果，就重来，避免错误的匹配一直传递下去
					if (0 && hint < VALID_MAX) {
						DispSize_t closest = INVALID_VAL;
						DispSize_t min_diff = VALID_MAX;
						int        best_count = 0;
						for (int idx = 0; idx < best_disp_num; idx++) {
							DispSize_t diff = diff_abs(hint, best_disp[idx]);
							if (diff < min_diff) {
								min_diff = diff;
								best_count = 1;
								closest = best_disp[idx];
							} else if (diff == min_diff) {
								best_count++;
								// 一左一右
								assert(closest + best_disp[idx] == 2 * hint);
							}
						}
						if (best_count == 1) {
							assert(closest < VALID_MAX);
							Disp(ldisp, i, j) = closest;
						}
						// else 不变

					} else {
						int min = best_disp[0];
						int max = min;
						for (int idx = 1; idx < best_disp_num; idx++) {
							if (best_disp[idx] > max)
								max = best_disp[idx];
							if (best_disp[idx] < min)
								min = best_disp[idx];
						}

						// 匹配结果相差比较大，说明可能有错误匹配，重来
						// 因为缩放的图和展开的图是不一样的，所以这种情况比较常见
						// 即lower层的结果在upper层属于错误匹配
						if (max - min <= best_disp_num)
							Disp(ldisp, i, j) = ((max + min) >> 1);
						else {
							// 使用lower层的结果，会提速，结果会平滑/模糊，但是 小物体会检测不到，
							// 更lower的层检测成本低，应该重新检测，如果有误差对upper层的影响非常大，
							if (level >= 1)
								Disp(ldisp, i, j) = INVALID_VAL;
						}
					}
				}
			} else {
				// 使用lower层的结果，会提速，结果会平滑，但是 小物体会检测不到，
				// 更lower的层检测成本低，应该重新检测，如果有误差对upper层的影响非常大，
				if (level >= 1)
					Disp(ldisp, i, j) = INVALID_VAL;
			}
		}
	}

	// 调试
#if 0
	Mat result;
	disp_to_opencv(result, 0);
	result *= (1<<level);
	cv::normalize(result, result, 255, 0, cv::NORM_MINMAX);
	cv::convertScaleAbs(result, result);

    applyColorMap(result, result, cv::COLORMAP_JET);
	imshow(std::to_string(level)+"disp", result);

	Mat gl,gr;
	grad_to_opencv(gl, gr);

	imshow(std::to_string(level)+"gradl", gl);
	imshow(std::to_string(level)+"gradr", gr);
	waitKey();
#endif
}

void PySgm::cost_aggr(std::unique_ptr<DispCost_t[]>& cost,
					  std::unique_ptr<DispCost_t[]>& aggr_x,
					  std::unique_ptr<DispCost_t[]>& aggr_y,
					  std::unique_ptr<uint8_t[]>&    image,
					  std::unique_ptr<DispCost_t[]>& sum) {
	auto img = [&](int i, int j, int k) -> int {
		return arr_access(image, height, width, i, j, k);
	};
	auto Heap = [&](std::unique_ptr<DispCost_t[]>& c, int i, int j) -> DispCost_t* {
		return &arr_access(c, width, cost_num, i, j, 0);
	};

	// 包含头
	auto copy_cost = [](DispCost_t* dst, const DispCost_t* src) {
		dst[0] = src[0];
		memcpy(dst + 1, src + 1, sizeof(*dst) * heap_size(src[0]));
	};

	// 变成 array [n] 形式的引用
#define Array(name, heap) auto& name = *(DispCost_t(*)[heap_size(*(heap))]) & (heap)[1]

	// 不包括边缘的聚合，x，y都能用，每条线聚合两次，第一次使用cost作为初始值，第二次直接累加
	auto aggr_common = [&](const DispCost_t* cost_head, std::unique_ptr<DispCost_t[]>& aggr, int i, int j, int i_last_offset, int j_last_offset, bool firsttime) {
		const Array(cost_arr, cost_head);

#ifndef NDEBUG
		int hs = heap_size(cost_head[0]);
		assert((int)(sizeof(cost_arr) / sizeof(cost_arr[0])) == hs);
#endif

		auto aggr_head = Heap(aggr, i, j);
		*aggr_head = *cost_head;
		auto aggr_pointer = aggr_head + 1;
		auto last_head = Heap(aggr, i + i_last_offset, j + j_last_offset);
		const Array(aggr_arr_last, last_head);

		// cost覆盖了上层的变量名
		for (auto& cost : cost_arr) {
			CostSize_t cost_d = COST_MAX, cost_dm1 = COST_MAX, cost_dp1 = COST_MAX;
			DispSize_t d = cost.disp;

			DispCost_t last_min;
			last_min.cost = COST_MAX;
			last_min.disp = (DispSize_t)-1;

			for (auto aggr_last : aggr_arr_last) {
				if (d == aggr_last.disp)
					cost_d = aggr_last.cost;
				else if (d == aggr_last.disp - 1)
					cost_dm1 = aggr_last.cost + P1;
				else if (d == aggr_last.disp + 1)
					cost_dp1 = aggr_last.cost + P1;

				if (aggr_last.cost < last_min.cost)
					last_min = aggr_last;
			}

			if (firsttime)
				*aggr_pointer = cost;

			if (last_min.cost != COST_MAX) {
				assert(last_min.disp != (DispSize_t)-1);
				int d_diff = diff_abs(d, last_min.disp);

				// 最小的可能有多个，只要和cost_d相同，就算视差相同，否则取那个视察就随缘了
				// diff 0 没有cost
				assert(cost_d >= last_min.cost);
				CostSize_t cost_diff = cost_d - last_min.cost;
				if (cost_diff <= 1) {
					// 如果是0，那么可以认为d_diff 为0
					// 如果是1,P1>=1 那么last_min.cost + P1 >= cost_d，最终结果也是+ cost_diff
					*aggr_pointer += cost_diff;
				} else {
					int        pixel_diff = diff_abs(img(0, i, j), img(0, i + i_last_offset, j + j_last_offset));
					CostSize_t cost_others = last_min.cost + calc_p2(d_diff, pixel_diff);
					*aggr_pointer += (std::min({cost_d, cost_dm1, cost_dp1, cost_others}) - last_min.cost);
				}
			}

			aggr_pointer++;
			assert(aggr_head->disp == (DispSize_t)-1);
			assert(aggr_pointer - (aggr_head + 1) <= heap_size(*aggr_head));
		}
	};

	for (int i = 0 + offset; i < height - offset; i++) {
		for (int j = 0 + offset; j < width - offset; j++) {
			const DispCost_t* cost_head = Heap(cost, i, j);

			// 左->右
			if (j == 0 + offset) {
				// 两者元素一样多，空间也能保证够用，直接复制
				auto aggr_x_head = Heap(aggr_x, i, j);
				aggr_x_head[0] = cost_head[0];
				aggr_x_head[1] = cost_head[1];
				assert(heap_size(*cost_head) <= 1 && cost_head->disp == (DispSize_t)-1);
				// d >0右边图像越界了，只有0是有意义的，即使分配了内存空间也不应该用。
				if (heap_size(*cost_head) == 1)
					assert(cost_head[1].disp == 0);
			} else {
				aggr_common(cost_head, aggr_x, i, j, 0, -1, true);
			}

			// 上 -> 下
			if (i == 0 + offset) {
				auto aggr_y_head = Heap(aggr_y, i, j);
				// 第一个不聚合，直接copy，但是边缘的视差有限制
				copy_cost(aggr_y_head, cost_head);
			} else {
				aggr_common(cost_head, aggr_y, i, j, -1, 0, true);
			}
		}

		// 右->左 第一个不聚合，也不需要copy cost，已经有了
		for (int j = width - offset - 2; j >= 0 + offset; j--) {
			const DispCost_t* cost_head = Heap(cost, i, j);
			aggr_common(cost_head, aggr_x, i, j, 0, 1, false);
		}
	}

	// 右图聚合结果初始化，右侧借用左侧结果，也放在这里了
	for (int i = 0 + offset; i < height - offset; i++) {
		for (int j = 0 + offset; j < width - offset; j++) {
			DispCost_t* raggr_sum_head = Heap(raggr_sum, i, j);
			heap_init(raggr_sum_head[0]);
		}
	}

	// 从下到上，顺便把上下左右的聚合结果加起来
	for (int i = height - offset - 1; i >= 0 + offset; i--) {
		for (int j = 0 + offset; j < width - offset; j++) {
			const DispCost_t* cost_head = Heap(cost, i, j);
			// 需要聚合
			if (i != height - offset - 1) {
				aggr_common(cost_head, aggr_y, i, j, 1, 0, false);
			}

			DispCost_t* sum_head = Heap(sum, i, j);
			DispCost_t* aggr_x_head = Heap(aggr_x, i, j);
			DispCost_t* aggr_y_head = Heap(aggr_y, i, j);

			// sum_head[0] = cost_head[0];
			//  sum可能和aggr_x共用内存，为了不干扰，使用了临时变量
			DispCost_t sum_tmp[cost_num];
			heap_init(sum_tmp[0]);

			// 最终结果, cost多加了一次，减去
			for (auto di = 1; di < heap_size(*cost_head) + 1; di++) {
				assert(cost_head[di].disp == aggr_x_head[di].disp && cost_head[di].disp == aggr_y_head[di].disp);
				// 可以不减，看两者比重的区分。
				DispCost_t c = aggr_x_head[di] + aggr_y_head[di] - cost_head[di];
				// 容量百分之百够用，使用小顶堆，小元素在前
				heap_insert(sum_tmp, cost_num, c, false);

				assert(j - c.disp >= 0);
				DispCost_t* raggr_sum_head = Heap(raggr_sum, i, j - c.disp);
				heap_insert(raggr_sum_head, 2, c, true);
			}
			assert(sum_tmp[0].cost == cost_head[0].cost);
			copy_cost(sum_head, sum_tmp);
		}
	}
}

// 左右上下导数，不是中间点的导数/梯度，有0.5偏移
void PySgm::grad_calc(void) {
	auto Img = [&](std::unique_ptr<uint8_t[]>& img, int i, int j) -> int {
		return arr_access(img, height, width, 0, i, j);
	};
	auto Grad = [&](std::unique_ptr<Grad_t[]>& grad, int xy, int i, int j) -> Grad_t& {
		return arr_access(grad, (height - 1), (width - 1), xy, i, j);
	};
	auto grad_fill = [&](std::unique_ptr<uint8_t[]>& img, std::unique_ptr<Grad_t[]>& g, int i, int j) {
		auto dx = Img(img, i, j + 1) - Img(img, i, j);
		auto dy = Img(img, i + 1, j) - Img(img, i, j);
		Grad(g, 0, i, j) = dx;
		Grad(g, 1, i, j) = dy;
	};
	for (int i = 0; i < height - 1 - 1; i++) {
		for (int j = 0; j < width - 1 - 1; j++) {
			grad_fill(l, lgrad, i, j);
			grad_fill(r, rgrad, i, j);
		}
	}
}

void PySgm::census_calc(void) {
	const int census_threshold = 3;

	auto img = [&](std::unique_ptr<uint8_t[]>& img, int i, int j) -> uint8_t {
		return arr_access(img, height, width, 0, i, j);
	};
#if 0
	auto grad = [&](std::unique_ptr<Grad_t[]> &grad, int xy, int i, int j) -> Grad_t {
		return arr_access(grad, (height-1), (width-1), xy, i, j);
	};
#endif
	auto census = [&](std::unique_ptr<Census_t[]>& c, int i, int j) -> Census_t& {
		return arr_access(c, height, width, 0, i, j);
	};
	auto census_fill = [&](std::unique_ptr<uint8_t[]>&  gray,
						   std::unique_ptr<Grad_t[]>&   g,
						   std::unique_ptr<Census_t[]>& c, int i, int j) {
		Census_t bits = 0;
		uint8_t  center = img(gray, i, j);
		for (int k = -offset; k < offset + 1; k++) {
			for (int l = -offset; l < offset + 1; l++) {
				if (k == 0 && l == 0)
					continue;

				// 这里使用分三部分的census
				if (img(gray, i + k, j + l) > center + census_threshold)
					bits |= 2; // 10
				else if (img(gray, i + k, j + l) + census_threshold < center)
					bits |= 1; // 01
				// else          // 00

				bits <<= 2;
			}
		}
#if 0
		// 扩充census的一种方案
		Grad_t u = grad(g, 1, i-1, j);
		Grad_t d = grad(g, 1, i, j);
		Grad_t l = grad(g, 0, i, j-1);
		Grad_t r = grad(g, 0, i, j);

		//    u
		//  l o r
		//    d
		//
		//    u < r < d < l => u < l， 所以有可能有一点冗余，不够独立，但也只是有可能，
		bits = (bits << 4) | ((r>u) << 3) | ((d>r) << 2) | ((l>d) << 1) | (u>l);
		assert(sizeof(Census_t)>= 2);
		assert((true << 1) == 2);
#endif

		census(c, i, j) = bits;
	};

	// 边缘:未初始化
	for (int i = offset; i < height - offset; i++) {
		for (int j = offset; j < width - offset; j++) {
			assert(offset >= 1);

			census_fill(l, lgrad, lcensus, i, j);
			census_fill(r, rgrad, rcensus, i, j);
		}
	}
}

void PySgm::load_from_opencv(const Mat& img0, const Mat& img1) {
	assert(img0.cols == img1.cols);
	assert(img0.rows == img1.rows);
	assert(img0.rows == height);
	assert(img0.cols == width);

	auto Value = [&](std::unique_ptr<uint8_t[]>& arr, int i, int j, int k) -> uint8_t& { return arr_access(arr, height, width, i, j, k); };

	for (int i = 0; i < height; i++) {
		auto ptr_l = img0.ptr<Vec3b>(i);
		auto ptr_r = img1.ptr<Vec3b>(i);
		for (int j = 0; j < width; j++) {
			Value(l, 0, i, j) = ptr_l[j][0];
			Value(l, 1, i, j) = ptr_l[j][1];
			Value(l, 2, i, j) = ptr_l[j][2];

			Value(r, 0, i, j) = ptr_r[j][0];
			Value(r, 1, i, j) = ptr_r[j][1];
			Value(r, 2, i, j) = ptr_r[j][2];
		}
	}
}

void PySgm::disp_to_opencv(Mat& result, int cut_offset) {
	auto Value = [&](int i, int j) -> DispSize_t& { return arr_access(ldisp, height, width, 0, i, j); };

	cout << height << "\t" << width << endl;
	if (height != result.rows || width != result.cols || result.depth() != CV_16U)
		result = Mat(height, width, CV_16SC1);

	for (int i = 0; i < height; i++) {
		auto ptr = result.ptr<int16_t>(i);
		for (int j = 0; j < width; j++) {
			auto disp = Value(i, j);
#if 1
			// 暂时把无效定义成0,实际上0和无效还是不同的，0代表无限远点
			if (disp >= VALID_MAX)
				ptr[j] = 0;
			else
#endif
				ptr[j] = disp + cut_offset;
		}
	}
}
void PySgm::grad_to_opencv(Mat& img0, Mat& img1) {
	auto Grad = [&](const std::unique_ptr<Grad_t[]>& arr, int i, int j, int k) -> Grad_t { return arr_access(arr, height - 1, width - 1, i, j, k); };

	cout << height << "\t" << width << endl;
	img0.create(height, width, CV_8UC1);
	img1.create(height, width, CV_8UC1);

	for (int i = 0; i < height - 1; i++) {
		auto ptr_l = img0.ptr<uchar>(i);
		auto ptr_r = img1.ptr<uchar>(i);
		for (int j = 0; j < width - 1; j++) {
			ptr_l[j] = abs(Grad(lgrad, 0, i, j)) + abs(Grad(lgrad, 1, i, j));

			ptr_r[j] = abs(Grad(rgrad, 0, i, j)) + abs(Grad(rgrad, 1, i, j));
		}
	}
}

void PySgm::img_to_opencv(Mat& img0, Mat& img1, int channel) {
	auto Image = [&](const std::unique_ptr<uint8_t[]>& arr, int i, int j, int k) -> uint8_t& { return arr_access(arr, height, width, i, j, k); };

	cout << height << "\t" << width << endl;
	img0.create(height, width, CV_8UC1);
	img1.create(height, width, CV_8UC1);

	for (int i = 0; i < height; i++) {
		auto ptr_l = img0.ptr<uchar>(i);
		auto ptr_r = img1.ptr<uchar>(i);
		for (int j = 0; j < width; j++) {
			ptr_l[j] = Image(l, channel, i, j);

			ptr_r[j] = Image(r, channel, i, j);
		}
	}
}

void PySgm::img_to_opencv(Mat& img0, Mat& img1) {
	auto Image = [&](const std::unique_ptr<uint8_t[]>& arr, int i, int j, int k) -> uint8_t& { return arr_access(arr, height, width, i, j, k); };

	cout << height << "\t" << width << endl;
	img0.create(height, width, CV_8UC3);
	img1.create(height, width, CV_8UC3);

	for (int i = 0; i < height; i++) {
		auto ptr_l = img0.ptr<Vec3b>(i);
		auto ptr_r = img1.ptr<Vec3b>(i);
		for (int j = 0; j < width; j++) {
			ptr_l[j][0] = Image(l, 0, i, j);
			ptr_l[j][1] = Image(l, 1, i, j);
			ptr_l[j][2] = Image(l, 2, i, j);

			ptr_r[j][0] = Image(r, 0, i, j);
			ptr_r[j][1] = Image(r, 1, i, j);
			ptr_r[j][2] = Image(r, 2, i, j);
		}
	}
}

// 假定lower层已经初始化过了
//
// 左侧woffset没有对应，除去woffset，边缘2*offset个没有对应，因为lower层没计算，
// 上下类似
//
// 下层边缘留一个不匹配，因为边缘信息不准，比如census，一个图中完整，一个不完整，这样匹配的结果是不可靠的
// 假设左和上offset 1, 左上角对lower层视差计算贡献的比例图如下
// 0 0 0 0 0 0 0 0
// 0 0 0 0 0 0 0 0
// 0 0 0 0 0 0 0 0
// 0 0 0 0 1 1 1 1
// 0 0 0 1 2 3 3 3
// 0 0 0 1 3 4 4 4
// 0 0 0 1 3 4 4 4
// 0 0 0 1 3 4 4 4
//
void PySgm::disp_upscale_from_next(int upper_disp_start, int upper_disp_end) {
	const auto H = this->height;
	const auto W = this->width;
	const int  border_size = 2 * offset + 1;

	assert(upper_disp_end - upper_disp_start > 4);

	//	int kernel[4][4] = {{0,1,1,0},{1,2,2,1},{1,2,2,1},{0,1,1,0}};
	// int kernel[4][4] = {{1,1,1,1},{1,1,1,1},{1,1,1,1},{1,1,1,1}};

	//	auto Range = [&](PySgm &sgm, int i, int j) -> DispSize_t (&)[2]{
	//		return *(DispSize_t(*)[2])&arr_access(sgm.ldisp_range, sgm.width, 2, i, j, 0);
	//	};
	auto Disp = [&](PySgm& sgm, int i, int j) -> DispSize_t& {
		return arr_access(sgm.ldisp, sgm.width, 1, i, j, 0);
	};

	// lower层之前的那一层边缘的一圈的视差要设置为invalid，这样代码上容易一些
	// 四周为0的地方没有计算，而是在计算cost取range的时候排除
	for (int I = this->hoffset + border_size; I < H - border_size; I++) {
		for (int J = this->woffset + border_size; J < W - border_size; J++) {
			// i, j主要成分， i2, j2次要成分
			int i = (I - this->hoffset - 1) / 2;
			int i2 = i - 1 + 2 * ((I - this->hoffset - 1) % 2);
			int j = (J - this->woffset - 1) / 2;
			int j2 = j - 1 + 2 * ((J - this->woffset - 1) % 2);

			DispSize_t d0 = Disp(*next, i, j);
			DispSize_t d1 = Disp(*next, i, j2);
			DispSize_t d2 = Disp(*next, i2, j);

			assert(diff_abs(i, i2) == 1);
			assert(diff_abs(j, j2) == 1);

			// 1 或者2, 2快一点
			// 但是range_get里面的step如果用2或4就会慢一些
			const int step = 1;
			// const int step = 1 + (level <= 1);
			//  相邻点差别太大无效，可能是突变点，不准
			/*else*/ if (d0 < VALID_MAX && diff_abs(d1, d0) <= step && diff_abs(d2, d0) <= step) {
#if 0
				// 计算平均值
				int num = 2, sum = d0<<1;
				if(d1 < VALID_MAX){
					sum += d1;
					num ++;
				}
				if(d2 < VALID_MAX){
					sum += d2;
					num ++;
				}
				// double，视差要扩大2倍
				d0 = 2*sum/num;
#else
				// 这样会快一点
				d0 <<= 1;
#endif
				if ((d0 > upper_disp_end))
					d0 = upper_disp_end;
				else if (unlikely(d0 < upper_disp_start))
					d0 = upper_disp_start;
				Disp(*this, I, J) = d0;

			} else {
				Disp(*this, I, J) = INVALID_VAL;
			}
		}
	}
}

void PySgm::downscale_to_next(void) {
	const auto H = height;
	const auto W = width;
	const auto h = next->height;
	const auto w = next->width;

	auto srcl = [&](int i, int j, int k) -> uint8_t { return arr_access(l, H, W, i, j, k); };
	auto srcr = [&](int i, int j, int k) -> uint8_t { return arr_access(r, H, W, i, j, k); };
	auto dstl = [&](int i, int j, int k) -> uint8_t& { return arr_access(next->l, h, w, i, j, k); };
	auto dstr = [&](int i, int j, int k) -> uint8_t& { return arr_access(next->r, h, w, i, j, k); };

	int kernel[4][4] = {{0, 1, 1, 0}, {1, 2, 2, 1}, {1, 2, 2, 1}, {0, 1, 1, 0}};

	for (int i = 0; i < h; i++) {
		for (int j = 0; j < w; j++) {
			int left[3] = {0, 0, 0};
			int right[3] = {0, 0, 0};
			for (int k = 0; k < 4; k++) {
				for (int l = 0; l < 4; l++) {
					left[0] += kernel[k][l] * srcl(0, hoffset + 2 * i + k, woffset + 2 * j + l);
					left[1] += kernel[k][l] * srcl(1, hoffset + 2 * i + k, woffset + 2 * j + l);
					left[2] += kernel[k][l] * srcl(2, hoffset + 2 * i + k, woffset + 2 * j + l);

					right[0] += kernel[k][l] * srcr(0, hoffset + 2 * i + k, woffset + 2 * j + l);
					right[1] += kernel[k][l] * srcr(1, hoffset + 2 * i + k, woffset + 2 * j + l);
					right[2] += kernel[k][l] * srcr(2, hoffset + 2 * i + k, woffset + 2 * j + l);
				}
			}

			// 7 舍 8 入
			dstl(0, i, j) = (8 + left[0]) / 16;
			dstl(1, i, j) = (8 + left[1]) / 16;
			dstl(2, i, j) = (8 + left[2]) / 16;

			dstr(0, i, j) = (8 + right[0]) / 16;
			dstr(1, i, j) = (8 + right[1]) / 16;
			dstr(2, i, j) = (8 + right[2]) / 16;
		}
	}
}

int main(int argc, char* argv[]) {
	if (argc < 3) {
		cout << "Usage: " << argv[0] << " left_image right_image [ disp_start  disp_end ]" << endl;
		return 1;
	}
	// test_heap();
	Mat l = imread(argv[1]);
	Mat r = imread(argv[2]);

	cv::cvtColor(l, l, cv::COLOR_BGR2YCrCb);
	cv::cvtColor(r, r, cv::COLOR_BGR2YCrCb);

	int disp_start = 0;
	int disp_end = 0;
	if (argc == 5) {
		disp_start = std::stoi(argv[3]);
		disp_end = std::stoi(argv[4]);
	}

	assert(l.cols == r.cols);
	assert(l.rows == r.rows);

	Mat result;
	int level = std::log2((l.rows + 2) / 12) + 1;
	cout << "level is " << level << endl;
	auto pysgm = PySgm(l.rows, l.cols, 8, 1, 16, 2, 5);
	// auto pysgm = PySgm(l.rows,l.cols, 8, 1, 16, 2, 1);
	if (disp_end != 0)
		pysgm.match(l, r, disp_start, disp_end, result, 0);
	else
		pysgm.match(l, r, 0, 2000, result, 0);

#if 0
	imshow("disp", result);
	waitKey();
	destroyAllWindows();

#endif

	cv::filterSpeckles(result, 0, 50, 1);

	// 16位结果，opencv读取png结果需要使用IMREAD_UNCHANGED等方式保证不被缩放
	imwrite("/tmp/result.png", result);
	imwrite("/tmp/result.bmp", result);
	// result.release();

	cv::normalize(result, result, 255, 0, cv::NORM_MINMAX);
	cv::convertScaleAbs(result, result);
	applyColorMap(result, result, cv::COLORMAP_JET);
	imshow("disp_color", result);
	waitKey();

	return 0;
}
